/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "modbus485driver.h"
#include <QDebug>
#include <QString>
#include "modbusdriverconfig.h"





Modbus485Driver::Modbus485Driver(QString fileName, int modbusIdIn,
                                 int *globalCurrentAliasIn, modbus_t *globalCurrentModbusMasterIn)
{
//    tcpVrep=NULL;
    isInitialedOk=false;
    modbusId=modbusIdIn;
    modbusMaster=NULL;
    globalCurrentAlias=globalCurrentAliasIn;
    globalCurrentModbusMaster=globalCurrentModbusMasterIn;

    controlWord=eServoOff;
    initial(fileName);

}

Modbus485Driver::~Modbus485Driver()
{
//    if(  tcpVrep!=NULL )
//    {
//        tcpVrep->close();
//    }
}

int Modbus485Driver::initial(QString fileName)
{
    qDebug()<<"Modbus485Driver initial ,file name="<<fileName;
    driverConfig=new ModbusDriverConfig(fileName);
    if(true==driverConfig->isInitialOkStatus())
    {
        driverConfig->getParameter(comParameter,regConfig,regAdd,vendorId);
        isInitialedOk=true;
        return 1;

    }
    return -1;
}

int Modbus485Driver::checkModbusMaster()
{
    if(E_MODBUS_TYPE_TCP==comParameter.modbusType)
    {
        return 1;
    }
    if(NULL==globalCurrentModbusMaster && -1==*globalCurrentAlias)
    {
        int returnKey;
        returnKey=connectPort();
        if(1==returnKey)
        {
            *globalCurrentAlias=modbusId;
            globalCurrentModbusMaster=modbusMaster;
            return 1;
        }
        else
        {
            return -1;
        }
    }
    else if(modbusId !=*globalCurrentAlias)
    {
        int returnKey;
        modbus_close(globalCurrentModbusMaster);
        modbus_free(globalCurrentModbusMaster);
        returnKey=connectPort();
        if(1==returnKey)
        {
            *globalCurrentAlias=modbusId;
            globalCurrentModbusMaster=modbusMaster;
            return 1;
        }
        else
        {
            return -2;
        }
    }

    return 1;
}

int Modbus485Driver::getModbusMaster(modbus_t* modbusMasterReturn)
{
    if(NULL==modbusMaster)
    {
        modbusMasterReturn=NULL;
        return -1;
    }
    else
    {
        modbusMasterReturn=modbusMaster;
    }
    return 1;
}

//int Modbus485Driver::setModbusMaster(modbus_t* modbusMasterIn)
//{
//    modbusMaster=modbusMasterIn;
//    modbus_set_slave(modbusMaster,modbusId);
//    return 1;
//}

int Modbus485Driver::servOnDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            controlWord=eServoEnable;
            break;
        }
    }


    return 1;
}

int Modbus485Driver::servOffDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            controlWord=eServoOff;
            break;
        }
    }


    return 1;
}

int Modbus485Driver::quickStopDriver()
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            controlWord=eServoQuickStop;
            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.speedControlAdd,0);
            if(1==resultKey)
            {
//                qDebug()<<"quickStopDriver:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }
            break;
        }
    }


    return 1;
}

int Modbus485Driver::setDriverMode(E_MOTOR_CONTROL_MODE modeIn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //0=pwn 1=torque-loop 2=velocity-loop 3=position-loop
            int tmpMode;
            if(E_MOTOR_CONTROL_CSP_MODE==modeIn)
            {
                tmpMode=3;
            }
            else if(E_MOTOR_CONTROL_CSV_MODE==modeIn)
            {
                tmpMode=2;
            }
            else if(E_MOTOR_CONTROL_CST_MODE==modeIn)
            {
                tmpMode=1;
            }
            else
            {
                return -2;
            }

            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.driverModelAdd,tmpMode);
            if(1==resultKey)
            {
                qDebug()<<"setDriverMode:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;

}

int Modbus485Driver::setPosition(int positionIn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //超时没有起作用，没发送数据的时候，没有停下来。
            uint16_t dataStream[2];
            dataStream[1] = positionIn & 0x0000ffff;
            dataStream[0] = ( positionIn & 0xffff0000 ) >> 16 ;
            //此处不支持写多个寄存器
//            int resultKey=modbus_write_registers(modbusMaster
//                                                ,regAdd.positionControlPositionHighAdd,2,dataStream);
            //write high value
            int resultKey=modbus_write_register(modbusMaster
                                                            ,regAdd.positionControlPositionHighAdd,dataStream[0]);

            if(-1!=resultKey)
            {
//                qDebug()<<"setPosition: modbus_write_register"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            //write low value
            resultKey=modbus_write_register(modbusMaster
                                                            ,regAdd.positionControlPositionLowAdd,dataStream[1]);

            if(-1!=resultKey)
            {
//                qDebug()<<"setPosition: modbus_write_register"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::setPwm(int pwmIn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.pwmControlAdd,pwmIn);
            if(1==resultKey)
            {
//                qDebug()<<"setPwm:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::setSpeed(int speedIn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
        //speedIn=２４时，反馈速度约１２
        //可以设置t=１S后通信中断时，驱动器停止电机
        //霍尔无刷电机，４极，１２puls/r, 换相频率(１hz=3puls/s)(１hz=1puls/s)
        //寄存器0x0035=1时，换相频率无倍率
        //寄存器0x0035=0时，换相频率x0.1
            if(controlWord!=eServoEnable)
            {
                speedIn=0;
            }
            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.speedControlAdd,10*speedIn);
            if(1==resultKey)
            {
//                qDebug()<<"setSpeed:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::setMaxCurrency(double ample)
{

    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.maxCurrencyAdd,ample*100);
            if(1==resultKey)
            {
//                qDebug()<<"setMaxCurrency:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}


int Modbus485Driver::setRatedCurrency(double ample)
{

    if( false==isInitialedOk)
    {
        return -1;
    }


    if(1!=checkModbusMaster())
    {
        return -100;
    }

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            int resultKey=modbus_write_register(modbusMaster
                                                ,regAdd.ratedCurrencyAdd,ample*100);
            if(1==resultKey)
            {
//                qDebug()<<"setRatedCurrency:modbus_write_register sucess"<<resultKey;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::changeDriverModbusId(int modbusId_new)
{
    //todo

    return 1;

}

int Modbus485Driver::getErrorCode( int &errorCodeReturn)
{

    if( false==isInitialedOk)
    {
        return -1;
    }


    if(1!=checkModbusMaster())
    {
        return -100;
    }

    uint16_t errorCode;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            int resultKey=modbus_read_registers(modbusMaster
                                                ,regAdd.errorCodeAdd,1,&errorCode);
            if(1==resultKey)
            {
//                qDebug()<<"getErrorCode:modbus_write_registers sucess"<<resultKey;
                errorCodeReturn=errorCode;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::setControlWord(int controlWordIn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            // to do
            controlWord=(E_MOTOR_CMD)controlWordIn;
            return 1;
        }
    }

    return -1;
}

int Modbus485Driver::getStatusWord(int& statusWordReturn)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            // to do
        //servo on state
        if(eServoEnable==controlWord)
        {
            statusWordReturn=eServoEnable;//与ethercat 一致
        }

        //servo off state
        if(eServoOff==controlWord)
        {
            statusWordReturn=eServoOff;//与ethercat 一致
        }

        //quick stop state
        if(eServoQuickStop==controlWord)
        {
            statusWordReturn=eServoQuickStop;//与ethercat 一致
        }

            return 1;
        }
    }

    return -1;
}

int Modbus485Driver::getPositionFeedback( int &positionFeedback)
{
    if( false==isInitialedOk)
    {
        return -1;
    }

    if(1!=checkModbusMaster())
    {
        return -100;
    }
    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            uint16_t dataStream[2];
            int resultKey=modbus_read_registers(modbusMaster
                                                ,regAdd.motorCurrentPositionFeedbackHighAdd,2,dataStream);
            if(-1!=resultKey)
            {
//                qDebug()<<"getPositionFeedback:modbus_read_registers sucess"<<resultKey;
                positionFeedback=dataStream[0]<<16;
                positionFeedback=positionFeedback+dataStream[1];
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;

}



int Modbus485Driver::getSpeedFeedback(double &speedFeedback)
{


    if( false==isInitialedOk)
    {
        return -1;
    }
    if(1!=checkModbusMaster())
    {
        return -100;
    }
    uint16_t speedFeedback_16;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            //霍尔无刷电机，４极，１２puls/r, 换相频率１hz=3puls/s １hz=1puls/s
            //寄存器0x0035=1时，换相频率无倍率
            //寄存器0x0035=0时，换相频率x0.1
            int resultKey=modbus_read_registers(modbusMaster
                                                ,regAdd.motorCurrentSpeedFeedbackAdd,1,&speedFeedback_16);
            if(1==resultKey)
            {
//                qDebug()<<"getSpeedFeedback:modbus_read_registers sucess"<<resultKey;
                int16_t *speedFeedback_s16=(int16_t*)&speedFeedback_16;
                speedFeedback=*speedFeedback_s16;
                speedFeedback=speedFeedback*0.1;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}


int Modbus485Driver::getCurrencyFeedback(double &currencyFeedback)
{

    if( false==isInitialedOk)
    {
        return -1;
    }
    if(1!=checkModbusMaster())
    {
        return -100;
    }
    uint16_t currencyFeedback_16;

    switch(vendorId)
    {
        case E_VENDOR_ID_AI_KONG:
        {
            int resultKey=modbus_read_registers(modbusMaster
                                                ,regAdd.motorCurrentCurrencyFeedbackAdd,1,&currencyFeedback_16);
            if(1==resultKey)
            {
//                qDebug()<<"getCurrencyFeedback:modbus_read_registers sucess"<<resultKey;
                currencyFeedback=(double)currencyFeedback_16/100.0;
                return 1;
            }
            else
            {
                return -1;
            }

            break;
        }
    }

    return -1;
}

int Modbus485Driver::initialDriverParameter()
{

    return 1;
}

bool Modbus485Driver::isInitialOkStatus()
{
    return isInitialedOk;
}

int Modbus485Driver::connectPort()
{


    if(E_MODBUS_TYPE_TCP==comParameter.modbusType)
    {
        modbusMaster = modbus_new_tcp(comParameter.ip.toStdString().data()
                                      ,comParameter.port);//由于是tcp client 连接，在同一程序中相同的端口可以连接多次
        modbus_set_slave(modbusMaster,modbusId);
        if (modbus_connect(modbusMaster) == -1)
        {
//            qDebug()<<"Modbus485Driver::connectPort connect failed";
            modbus_free(modbusMaster);
            modbusMaster=NULL;
            return -2;
        }
        else
        {
            modbus_set_response_timeout(modbusMaster, 0, 500000);
//            qDebug()<<"connect success";
            return 1;
        }
    }
    else if(E_MODBUS_TYPE_RTU==comParameter.modbusType)
    {

        modbusMaster =modbus_new_rtu(comParameter.serialDeviceName.toStdString().data(),
                                     comParameter.baud, (*comParameter.parity.toStdString().data()),
                                            comParameter.data_bit, comParameter.stop_bit);
        modbus_set_slave(modbusMaster,modbusId);
        if (modbus_connect(modbusMaster) == -1)
        {
//           qDebug()<<"Modbus485Driver::connectPort connect failed";
            modbus_free(modbusMaster);
            modbusMaster=NULL;
            return -2;
        }
        else
        {
            modbus_set_response_timeout(modbusMaster, 0, 500000);
//            qDebug()<<"connect success";
            return 1;
        }
    }
    return -1;
}




